#include "ina226.h"
#include <math.h>
#include "delay_us.h"
#include "stm32f1xx_ll_gpio.h"
#include "FreeRTOS.h"
#include "semphr.h"

// I2C总线互斥锁，确保同一时间只有一个任务访问I2C总线
static SemaphoreHandle_t i2c_mutex = NULL;

/* 私有函数声明 */
static uint8_t I2C_Start(INA226_Dev *dev);
static void I2C_Stop(INA226_Dev *dev);
static uint8_t I2C_WriteByte(INA226_Dev *dev, uint8_t byte);
static uint8_t I2C_ReadByte(INA226_Dev *dev, uint8_t ack);
static uint8_t WriteReg(INA226_Dev *dev, uint8_t reg, uint16_t value);
static uint8_t ReadReg(INA226_Dev *dev, uint8_t reg, uint16_t *value);
static void I2C_Recovery(INA226_Dev *dev); // I2C总线恢复函数
extern uint8_t aa;
#define I2C_TIMEOUT 1000  // 1ms超时时间
#define MAX_RETRIES 3     // 最大重试次数



/* I2C总线恢复函数 - 当I2C总线被锁定时尝试恢复 */
static void I2C_Recovery(INA226_Dev *dev)
{
    // 确保SCL为输出模式
    LL_GPIO_SetOutputPin(dev->scl_port, dev->scl_pin);
    LL_GPIO_SetOutputPin(dev->sda_port, dev->sda_pin);
    
    // 发送9个时钟脉冲，帮助从设备释放总线
    for(uint8_t i = 0; i < 9; i++) {
        delay_us(5);
        LL_GPIO_SetOutputPin(dev->scl_port, dev->scl_pin);
        delay_us(5);
        LL_GPIO_ResetOutputPin(dev->scl_port, dev->scl_pin);
    }
    
    // 发送停止条件
    I2C_Stop(dev);
    
    // 等待总线稳定
    delay_us(10);
}

/* I2C起始信号 */
static uint8_t I2C_Start(INA226_Dev *dev)
{
    // 确保GPIO为输出模式
    LL_GPIO_SetOutputPin(dev->sda_port, dev->sda_pin);
    LL_GPIO_SetOutputPin(dev->scl_port, dev->scl_pin);
    delay_us(10);  // 确保总线空闲
    
    // 检查总线是否忙
    if(LL_GPIO_IsInputPinSet(dev->sda_port, dev->sda_pin) == 0) {
        // 总线被占用，尝试恢复
        I2C_Recovery(dev);
        
        // 再次检查总线
        if(LL_GPIO_IsInputPinSet(dev->sda_port, dev->sda_pin) == 0) {
            return INA226_ERR_START;
        }
    }
    
    // 发送起始信号
    LL_GPIO_ResetOutputPin(dev->sda_port, dev->sda_pin);
    delay_us(5);  // 确保建立时间
    LL_GPIO_ResetOutputPin(dev->scl_port, dev->scl_pin);
    return INA226_OK;
}

/* I2C停止信号 */
static void I2C_Stop(INA226_Dev *dev)
{
    // 确保SDA为低
    LL_GPIO_ResetOutputPin(dev->sda_port, dev->sda_pin);
    delay_us(5);
    
    // SCL设为高
    LL_GPIO_SetOutputPin(dev->scl_port, dev->scl_pin);
    delay_us(5);  // 确保建立时间
    
    // SDA从低到高，产生停止信号
    LL_GPIO_SetOutputPin(dev->sda_port, dev->sda_pin);
    delay_us(5);  // 确保保持时间
}

/* 写入字节 */
static uint8_t I2C_WriteByte(INA226_Dev *dev, uint8_t byte)
{
    for(uint8_t i=0; i<8; i++){
        // 设置数据位
        (byte & 0x80) ? LL_GPIO_SetOutputPin(dev->sda_port, dev->sda_pin) :
                        LL_GPIO_ResetOutputPin(dev->sda_port, dev->sda_pin);
        byte <<= 1;
        delay_us(2);  // 数据建立时间
        
        // 产生时钟脉冲
        LL_GPIO_SetOutputPin(dev->scl_port, dev->scl_pin);
        delay_us(5);  // 时钟高电平时间
        LL_GPIO_ResetOutputPin(dev->scl_port, dev->scl_pin);
        delay_us(5);  // 时钟低电平时间
    }

    // 读取ACK
    LL_GPIO_SetOutputPin(dev->sda_port, dev->sda_pin);
    delay_us(2);
    
    LL_GPIO_SetOutputPin(dev->scl_port, dev->scl_pin);
    delay_us(2);  // 确保建立时间
    
    uint32_t timeout = I2C_TIMEOUT;
    while(LL_GPIO_IsInputPinSet(dev->sda_port, dev->sda_pin)){
        if(--timeout == 0){
            LL_GPIO_ResetOutputPin(dev->scl_port, dev->scl_pin);
            return INA226_ERR_TIMEOUT;
        }
        delay_us(1);
    }
    
    LL_GPIO_ResetOutputPin(dev->scl_port, dev->scl_pin);
    return INA226_OK;
}

/* 读取字节 */
static uint8_t I2C_ReadByte(INA226_Dev *dev, uint8_t ack)
{
    uint8_t byte = 0;
    
    // 设置SDA为输入模式（释放总线）
    LL_GPIO_SetOutputPin(dev->sda_port, dev->sda_pin);
    
    for(uint8_t i=0; i<8; i++){
        byte <<= 1;
        
        // 产生时钟脉冲
        LL_GPIO_SetOutputPin(dev->scl_port, dev->scl_pin);
        delay_us(5);  // 时钟高电平时间，确保数据稳定
        
        // 读取数据位
        if(LL_GPIO_IsInputPinSet(dev->sda_port, dev->sda_pin))
            byte |= 0x01;
        
        LL_GPIO_ResetOutputPin(dev->scl_port, dev->scl_pin);
        delay_us(5);  // 时钟低电平时间
    }
    
    // 发送ACK/NACK
    ack ? LL_GPIO_ResetOutputPin(dev->sda_port, dev->sda_pin) :
          LL_GPIO_SetOutputPin(dev->sda_port, dev->sda_pin);
    delay_us(2);  // 确保数据建立时间
    
    LL_GPIO_SetOutputPin(dev->scl_port, dev->scl_pin);
    delay_us(5);  // 时钟高电平时间
    LL_GPIO_ResetOutputPin(dev->scl_port, dev->scl_pin);
    
    // 释放SDA
    LL_GPIO_SetOutputPin(dev->sda_port, dev->sda_pin);
    
    return byte;
}

/* д�Ĵ��� */
static uint8_t WriteReg(INA226_Dev *dev, uint8_t reg, uint16_t value)
{
    uint8_t res;
    if((res = I2C_Start(dev)) != INA226_OK) return res;
    
    // �����豸��ַ��д��
    if((res = I2C_WriteByte(dev, (dev->addr << 1) | 0)) != INA226_OK)
        return INA226_ERR_ADDR;
    
    // ���ͼĴ�����ַ
    if((res = I2C_WriteByte(dev, reg)) != INA226_OK)
        return INA226_ERR_REG;
    
    // д������
    if((res = I2C_WriteByte(dev, (value >> 8) & 0xFF)) != INA226_OK)
        return INA226_ERR_DATA;
    if((res = I2C_WriteByte(dev, value & 0xFF)) != INA226_OK)
        return INA226_ERR_DATA;
    
    I2C_Stop(dev);
    return INA226_OK;
}

/* ���Ĵ��� */
static uint8_t ReadReg(INA226_Dev *dev, uint8_t reg, uint16_t *value)
{
    uint8_t res;
    if((res = I2C_Start(dev)) != INA226_OK) return res;
    
    // �����豸��ַ��д��
    if((res = I2C_WriteByte(dev, (dev->addr << 1) | 0)) != INA226_OK)
        return INA226_ERR_ADDR;
    
    // ���ͼĴ�����ַ
    if((res = I2C_WriteByte(dev, reg)) != INA226_OK)
        return INA226_ERR_REG;
    
    // ��������
    if((res = I2C_Start(dev)) != INA226_OK) return res;
    
    // �����豸��ַ������
    if((res = I2C_WriteByte(dev, (dev->addr << 1) | 1)) != INA226_OK)
        return INA226_ERR_ADDR;
    
    // ��ȡ����
    uint8_t high = I2C_ReadByte(dev, 1); // ACK
    uint8_t low = I2C_ReadByte(dev, 0);  // NACK
    *value = (high << 8) | low;
    
    I2C_Stop(dev);
    return INA226_OK;
}

/* ��ʼ���豸 */
uint8_t INA226_Init(INA226_Dev *dev)
{
    // 初始化互斥锁（如果尚未初始化）
    if(i2c_mutex == NULL) {
        i2c_mutex = xSemaphoreCreateMutex();
        if(i2c_mutex == NULL) {
            return INA226_ERR_CALIB; // 互斥锁创建失败
        }
    }
    
    // 获取互斥锁
    if(xSemaphoreTake(i2c_mutex, portMAX_DELAY) != pdTRUE) {
        return INA226_ERR_CALIB; // 无法获取互斥锁
    }
    
    // 计算校准值
    dev->current_lsb = dev->max_current / 32768.0f;
    float cal_float = 0.00512f / (dev->current_lsb * dev->shunt_r);
    if(cal_float > 0xFFFF) {
        xSemaphoreGive(i2c_mutex);
        return INA226_ERR_CALIB;
    }
    dev->cal_value = (uint16_t)cal_float;
    
    // 配置寄存器(平均值64次转换时间4.1ms)
    uint8_t res = WriteReg(dev, 0x00, 0x47FF);
	
    if(res != INA226_OK) {
        xSemaphoreGive(i2c_mutex);
        return res;
    }
    
    // 写入校准寄存器
    res = WriteReg(dev, 0x05, dev->cal_value);
    
    // 释放互斥锁
    xSemaphoreGive(i2c_mutex);
    
    return res;
}

/* 读取所有数据 */
uint8_t INA226_ReadAll(INA226_Dev *dev, float *bus_v, float *current, float *shunt_v)
{
    uint16_t raw;
    uint8_t res;
    uint8_t retry_count = 0;
    
    // 确保互斥锁已初始化
    if(i2c_mutex == NULL) {
        i2c_mutex = xSemaphoreCreateMutex();
        if(i2c_mutex == NULL) {
            return INA226_ERR_CALIB; // 互斥锁创建失败
        }
    }
    
    // 尝试获取互斥锁，使用较长的超时时间以确保能获取到锁
    if(xSemaphoreTake(i2c_mutex, 500 / portTICK_PERIOD_MS) != pdTRUE) {
        // 获取锁失败，尝试恢复I2C总线
        I2C_Recovery(dev);
        
        // 再次尝试获取锁
        if(xSemaphoreTake(i2c_mutex, 500 / portTICK_PERIOD_MS) != pdTRUE) {
            return INA226_ERR_RANGE; // 无法获取互斥锁
        }
    }
    
    // 带重试机制的读取
    while(retry_count < MAX_RETRIES) {
        // 读取总线电压(0x02)
        if((res = ReadReg(dev, 0x02, &raw)) != INA226_OK) {
            retry_count++;
            if(retry_count >= MAX_RETRIES) {
                xSemaphoreGive(i2c_mutex);
                return res;
            }
            // 失败后尝试恢复总线并等待一小段时间
            I2C_Recovery(dev);
            delay_us(100);
            continue;
        }
        *bus_v = raw * 1.25e-3f; // LSB=1.25mV
        
        // 读取分流电压(0x01)
        if((res = ReadReg(dev, 0x01, &raw)) != INA226_OK) {
            retry_count++;
            if(retry_count >= MAX_RETRIES) {
                xSemaphoreGive(i2c_mutex);
                return res;
            }
            // 失败后尝试恢复总线并等待一小段时间
            I2C_Recovery(dev);
            delay_us(100);
            continue;
        }
        *shunt_v = (int16_t)raw * 2.5e-6f; // LSB=2.5μV
        
        // 计算电流
        *current = *shunt_v / dev->shunt_r;
        
        // 数据范围验证
        if(fabs(*current) > dev->max_current * 2.0f) { // 扩大范围检查，允许更大的误差
            retry_count++;
            if(retry_count >= MAX_RETRIES) {
                xSemaphoreGive(i2c_mutex);
                return INA226_ERR_RANGE;
            }
            // 数据异常，尝试恢复总线并等待一小段时间
            I2C_Recovery(dev);
            delay_us(100);
            continue;
        }
        
        // 成功读取，退出循环
        break;
    }
    
    // 释放互斥锁
    xSemaphoreGive(i2c_mutex);
    
    // 检查是否所有重试都失败了
    if(retry_count >= MAX_RETRIES) {
        return INA226_ERR_TIMEOUT;
    }
    
    return INA226_OK;
}